/*
 *
 * Copyright (C) 2014
 * Julio Jarquin, Gustavo Arechavaleta <garechav@cinvestav.edu.mx>
 * CINVESTAV - Saltillo Campus
 *
 * This file is part of HRLocomotion-1.0
 * HRLocomotion-1.0 free software; you can redistribute it and/or
 * modify it under the terms of the GNU Lesser General Public
 * License as published by the Free Software Foundation; either
 * version 2.1 of the License, or (at your option) any later version.
 *
 * HRLocomotion-1.0 is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
 * Lesser General Public License for more details.

 * You should have received a copy of the GNU Lesser General Public
 * License along with this library; if not, write to the Free Software
 * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA
 * 02110-1301  USA
 */

/**
 *	\file src/mpc_data.h
 *	\author Julio Jarquin, Gustavo Arechavaleta
 *	\version 1.0
 *	\date 2015
 *
 *	Declaration of the MPCData class.
 *  The MPCData class holds and initialize the Hessian matrix and the vector matrix
 *  associated with the quadratic program.
 */

#ifndef HR_LOCOMOTION_MPC_DATA_H
#define HR_LOCOMOTION_MPC_DATA_H

#include "HRLocomotion/hr_types.h"
#include "robot_state.h"
#include <vector>

namespace hrLocomotion{



class MPCData
{
    friend class MPCBounds;   // MPCBounds can access to the private members of class MPCData
    friend class MPCConstraints; // MPCConstraints can access to the private members of class MPCData
public:


    MPCData():
        g(9.80665), h_com(0.315), alpha(10e-6), beta(1), gamma(1), T_s(0.1), m_steps(2), s_stage(8), stage(0),N_horizon(s_stage*m_steps){}

    MPCData(const real_t& com_height):
        g(9.80665), h_com(com_height), alpha(10e-6), beta(1), gamma(1), T_s(0.1), m_steps(2), s_stage(8), stage(0),N_horizon(s_stage*m_steps){}

    MPCData(const real_t& com_height, const real_t& T_sampling):
        g(9.80665), h_com(com_height), alpha(10e-6), beta(1), gamma(1), T_s(T_sampling), m_steps(2), s_stage(8), stage(0),N_horizon(s_stage*m_steps){}

    MPCData(const real_t& com_height, const real_t& T_sampling, const real_t& kalpha, const real_t& kbeta, const real_t& kgamma):
        g(9.80665), h_com(com_height), alpha(kalpha), beta(kbeta), gamma(kgamma), T_s(T_sampling), m_steps(2), s_stage(8), stage(0),N_horizon(s_stage*m_steps){}

    MPCData(const real_t& com_height, const real_t& T_sampling, VectorXr gains):
            g(9.80665), h_com(com_height), alpha(gains(0)), beta(gains(1)), gamma(gains(2)), T_s(T_sampling),
                                                        m_steps(2), s_stage(8), stage(0),N_horizon(s_stage*m_steps){}

    MPCData(const real_t& com_height, const real_t& T_sampling, const real_t& kalpha, const real_t& kbeta, const real_t& kgamma,
            const int& stepHorizon, const int& singleSupportPeriods):
        g(9.80665), h_com(com_height), alpha(kalpha), beta(kbeta), gamma(kgamma), T_s(T_sampling),
                                                        m_steps(stepHorizon), s_stage(singleSupportPeriods), stage(0),N_horizon(s_stage*m_steps){}

    MPCData(const real_t& com_height, const real_t& T_sampling, VectorXr gains, const int& stepHorizon, const int& singleSupportPeriods):
        g(9.80665), h_com(com_height), alpha(gains(0)), beta(gains(1)), gamma(gains(2)), T_s(T_sampling),
                                                    m_steps(stepHorizon), s_stage(singleSupportPeriods), stage(0),N_horizon(s_stage*m_steps){}

    MPCData(const real_t& com_height, const real_t& T_sampling, const int& stepHorizon, const int& singleSupportPeriods):
        g(9.80665), h_com(com_height), alpha(10e-6), beta(1), gamma(1), T_s(T_sampling), m_steps(stepHorizon),
                                                        s_stage(singleSupportPeriods), stage(0),N_horizon(s_stage*m_steps){}

    MPCData(const real_t& com_height, const real_t& T_sampling, const int& stepHorizon, const int& singleSupportPeriods, VectorXr gains):
            g(9.80665), h_com(com_height), alpha(gains(0)), beta(gains(1)), gamma(gains(2)), T_s(T_sampling),
                                                        m_steps(stepHorizon), s_stage(singleSupportPeriods), stage(0),N_horizon(s_stage*m_steps){}


    MPCData(const real_t& com_height, const real_t& T_sampling, const int& stepHorizon, const int& singleSupportPeriods,
            const real_t& kalpha, const real_t& kbeta, const real_t& kgamma):
        g(9.80665), h_com(com_height), alpha(kalpha), beta(kbeta), gamma(kgamma), T_s(T_sampling),
                                                        m_steps(stepHorizon), s_stage(singleSupportPeriods), stage(0),N_horizon(s_stage*m_steps){}


    MPCData& operator=(const MPCData &);

    void init();
    int getStage(){return stage;}
    void next();
    MatrixXr getHessian();
    VectorXr getFvector(RobotState state,VectorXr footState, VectorXr dx_ref, VectorXr dy_ref);


#ifdef _COMPILE_DEBUGGING_METHODS
    void    printConstants();
#endif //_COMPILE_DEBUGGING_METHODS
protected:

private:
    ///MPC Constants

    const real_t g;      /** Gravity constant */
    const real_t h_com;      /** CoM fixed heigh */
    const real_t alpha;         /** Jerk gain */
    const real_t beta;         /** Velocity gain */
    const real_t gamma;        /** ZMP gain */
    const real_t T_s;           /** Double support period */

    unsigned int m_steps;      /** Future m steps to be considered */
    unsigned int s_stage;     /** Duration of single support stage in Periods T_s. Double support stage < T_s */
    unsigned int N_horizon;      /** Previewable window in periods T_s */

    int stage; /** Current stage for the varying matrix Q*/

    MatrixXr Pvs;        /** Mtrices P of lenght N, are the recursivity of dynamics,  */
    MatrixXr Pvu;
    MatrixXr Pzs;
    MatrixXr Pzu;
    std::vector<VectorXr> Uc;    /** Container of selection vectors Uc, current ZMP reference */
    std::vector<MatrixXr> Ur;    /** Container of selection matrices Ur, next ZMP reference */
    std::vector<MatrixXr> Q;     /** Container of matrices Q, ------ */


    VectorXr pk;         /** vector of N size, ------ */

    void initMatricesP();    /** Needed by Q and Pk */
    void initMatricesU();    /** Needed by Q and Pk*/

    void initHessian();    //Q for QP Hessian Matrix


};

}

#endif // MPC_DATA_H
